#include "MyLine3D.h"
#include "GraphFitting.h"
//int MyLine3D::W=0;
//int MyLine3D::H=0;
//int MyLine3D::S=0;
bool MyLine3D::useShortLines=false;
bool MyLine3D::finiteSearch=false;
bool MyLine3D::exhaustiveSearch=false;
extern CImageData* pImg3D;
extern CGraphFitting *pGf;
MyLine3D::MyLine3D()
{
	xa=ya=za=0;
	xb=yb=zb=10;
	theta=1;
	mRadius=-1;
	goodness=1.0;
	lambda=1.0;
}
MyLine3D::~MyLine3D(void)
{
}
void MyLine3D::initCrossBox()
{
	osg::Vec3f tn(xa-xb,ya-yb,za-zb);
	if(tn.x()==0&&tn.y()==0)	tn.y()=1;
	osg::Vec3f tu(tn.y(),-tn.x(),0);
	osg::Vec3f tv(tn.x()*tn.z(),tn.y()*tn.z(),-(tn.x()*tn.x()+tn.y()*tn.y()) );
	tu.normalize();
	tv.normalize();
	nu=tu;
	nv=tv;
}
bool MyLine3D::sameDirect(const Voxel& va,const Voxel& vb,const CImageData& img3D)
{
	//return true;
#ifdef BIG_CUBE
	return true;
#endif
	Vessel vla=img3D.getVessel(va.x,va.y,va.s);
	Vessel vlb=img3D.getVessel(vb.x,vb.y,vb.s);

	const double minVal=cos(osg::DegreesToRadians(15.0));
	double val=getAngle(cv::Point3f(vla.data[1],vla.data[2],vla.data[3] ),cv::Point3f(vlb.data[1],vlb.data[2],vlb.data[3]));
	if(val<minVal)	return false;

	val=getAngle(cv::Point3f(vla.data[1],vla.data[2],vla.data[3] ),cv::Point3f(vb.x-va.x,vb.y-va.y,vb.s-va.s));
	if(val<minVal)	return false;

	val=getAngle(cv::Point3f(vlb.data[1],vlb.data[2],vlb.data[3] ),cv::Point3f(vb.x-va.x,vb.y-va.y,vb.s-va.s));
	if(val<minVal)	return false;

	return true;
}
void MyLine3D::sampleLine(CImageData& img3D)
{
	srand((unsigned int)time(0));
	do{
		xa=rand()%W;
		ya=rand()%H;
		za=rand()%S;
		if(!img3D.isVessel(xa,ya,za))	continue;

		if(img3D.pDense3D->get(xa,ya,za)>1)	img3D.pDense3D->get(xa,ya,za)--;
		else if(img3D.pDense3D->get(xa,ya,za)==1)
		{
			int size=10;
			std::vector<Voxel> nearbyPoints; 
			for(int mz=-size;mz<=size;++mz)
				for(int my=-size;my<=size;++my)
					for(int mx=-size;mx<=size;++mx)
					{
						Voxel vb(xa+mx,ya+my,za+mz);
						if(inRange(vb) && img3D.isVessel(vb.x,vb.y,vb.s))
						{
							nearbyPoints.push_back(vb);
						}
					}
			/////////////////////////////////////////////////////////
			Voxel va(xa,ya,za);
			bool found=false;
			for(int m=0;m<1000;++m)
			{
				int id=rand()%nearbyPoints.size();
				Voxel vb=nearbyPoints[id];
				xb=vb.x; yb=vb.y; zb=vb.s;
				if(inRange(vb)&&validLine()&&img3D.isVessel(vb.x,vb.y,vb.s)&&getLength()>=MIN_LEN&&getLength()<=MAX_LEN&&sameDirect(va,vb,img3D))		
				{
					found=true;
					break;
				}
			}
			img3D.pDense3D->get(xa,ya,za)=0;//will not be used again
			if(found)	break;
		}
	}
	while(true);
}
//double MyLine3D::compEnergy(int x,int y,int z,const CImageData& img3D) const
//{
//	Vessel vl=img3D.getVessel(x,y,z);
//	double dis=compDist(x,y,z);
//	dis*=dis;
//
//	double energy=//dis/theta; 
//					log(sqrt(2*osg::PI)*theta) +dis/(theta*theta);
//	if(energy<-500)	energy=-500;
//	if(energy>500)	energy=500;
//
//	return energy;;//*getLength();
//}
void MyLine3D::displayEnergy()
{
	for(int i=0;i<supportPoints.size();++i)
	{
		double energy= compEnergy(supportPoints[i].x,supportPoints[i].y,supportPoints[i].z);
		printf("%.2f ",energy);
	}
	printf("\n");
}
double MyLine3D::compEnergy(int x,int y,int z) const
{
	const int THRES_INT=50000;
	double dis=compDist(x,y,z);
	dis*=dis;

	double energy=log(sqrt(2*osg::PI)*theta) +dis/(theta*theta);
	//	log(4*osg::PI*theta*theta)
	//	+log(getLength())+dis/(2*theta*theta);
	if(energy<-500)	energy=-500;
	if(energy>500)	energy=500;
	return energy;

	// the following is line segment model 
	cv::Point3f Vab(xa-xb,ya-yb,za-zb);
	cv::Point3f Vpb(x-xb,y-yb,z-zb);
	cv::Point3f Vpa(x-xa,y-ya,z-za);

	double M1=erf(Vab.dot(Vpb)/(1.414*theta*getLength()));
	double M2=erf(Vab.dot(Vpa)/(1.414*theta*getLength()));
	if(M1-M2<0.0001)
	{
		return THRES_INT;
		//printf("%.2f,%.2f,%g\n",M1,M2,M1-M2);
		//exit(2);
	}
	else
	{
		energy-=log(M1-M2);
	}
	//printf("%.2f ",energy);
	if(energy<-THRES_INT)	energy=-THRES_INT;
	if(energy>THRES_INT)	energy=THRES_INT;


//	Vessel vl=pImg3D->getVessel(x,y,z);
//	double angle=compAngle(vl.data[1],vl.data[2],vl.data[3]);
	return energy;
}

double MyLine3D::compDist(float xc,float yc,float zc) const
{
	double SAB=sqrt(0.0+ (xa-xb)*(xa-xb)+(ya-yb)*(ya-yb)+(za-zb)*(za-zb)  );
	double SAC=sqrt(0.0+ (xa-xc)*(xa-xc)+(ya-yc)*(ya-yc)+(za-zc)*(za-zc)  );
	double SBC=sqrt(0.0+ (xc-xb)*(xc-xb)+(yc-yb)*(yc-yb)+(zc-zb)*(zc-zb)  );
	double P=(SAB+SAC+SBC)/2.0;
	double SS=sqrt(P*(P-SBC)*(P-SAC)*(P-SAB));

	cv::Point3f v21(xc-xa,yc-ya,zc-za);
	cv::Point3f v23(xb-xa,yb-ya,zb-za);
	double angel=getAngle(v21,v23);
	if(angel<cos(ANGLE))//angel cab >90,then dist=ca
	{
		return sqrt(v21.dot(v21));
	}
	else 
	{
		v21=cv::Point3f(xc-xb,yc-yb,zc-zb);
		v23=cv::Point3f(xa-xb,ya-yb,za-zb);
		angel=getAngle(v21,v23);
		if(angel<cos(ANGLE))	//angel cba >90,then dist=cb
		{
			return sqrt(v21.dot(v21));
		}
	}
	return (SS+SS)/SAB;
}
bool MyLine3D::inLineRange(int x,int y,int z)
{
	cv::Point3f v21(x-xa,y-ya,z-za);
	cv::Point3f v23(xb-xa,yb-ya,zb-za);
	double angel=getAngle(v21,v23);
	if(angel<cos(ANGLE))//angel cab >90
		return false;

	v21=cv::Point3f(x-xb,y-yb,z-zb);
	v23=cv::Point3f(xa-xb,ya-yb,za-zb);
	angel=getAngle(v21,v23);
	if(angel<cos(ANGLE))//angel cba >90
		return false;

	return compDirectDist(x,y,z)<mRadius;
}
cv::Point3f MyLine3D::getProjection(int x,int y,int z) const
{
	cv::Point3f v21(x-xa,y-ya,z-za);
	cv::Point3f v23(xb-xa,yb-ya,zb-za);
	double angel=getAngle(v21,v23);
	if(angel<cos(ANGLE))//angel cab >90
		return cv::Point3f(xa,ya,za);

	v21=cv::Point3f(x-xb,y-yb,z-zb);
	v23=cv::Point3f(xa-xb,ya-yb,za-zb);
	angel=getAngle(v21,v23);
	if(angel<cos(ANGLE))//angel cba >90
		return cv::Point3f(xb,yb,zb);

	double dis=compDirectDist(x,y,z);
	double steps=sqrt(0.0+(x-xa)*(x-xa)+(y-ya)*(y-ya)+(z-za)*(z-za)-(dis*dis));
	double rate=steps/getLength();
	cv::Point3f diff((xb-xa)*rate,(yb-ya)*rate,(zb-za)*rate);
	diff+=cv::Point3f(xa,ya,za);

//	double offset=compDist(diff.x,diff.y,diff.z);
//	printf("%.5lf ",offset);
	return diff;
}
double MyLine3D::compDirectDist(float xc,float yc,float zc) const
{
	double SAB=sqrt(0.0+ (xa-xb)*(xa-xb)+(ya-yb)*(ya-yb)+(za-zb)*(za-zb)  );
	double SAC=sqrt(0.0+ (xa-xc)*(xa-xc)+(ya-yc)*(ya-yc)+(za-zc)*(za-zc)  );
	double SBC=sqrt(0.0+ (xc-xb)*(xc-xb)+(yc-yb)*(yc-yb)+(zc-zb)*(zc-zb)  );
	double P=(SAB+SAC+SBC)/2.0;

	if(P*(P-SBC)*(P-SAC)*(P-SAB)<0.0001)
		return 0;

	double SS=sqrt(P*(P-SBC)*(P-SAC)*(P-SAB));

//	if(SAB<0.001)	SAB=0.001;
	return (SS+SS)/SAB;
}
double  MyLine3D::compAngle(const MyLine3D& line) const
{
	Voxel vl=line.getDirect();
	return compAngle(vl.x,vl.y,vl.s);
}
double MyLine3D::compAngle(float tx,float ty,float tz) const
{
	double angle=getAngle(cv::Point3f(xa-xb,ya-yb,za-zb),cv::Point3f(tx,ty,tz));
	return abs(angle);
}
void MyLine3D::createLine(int x,int y,int z)
{
	setStartPoint(x,y,z);

	while(xa==xb&&ya==yb&&za==zb)
	{
		xb=rand()%W;
		yb=rand()%H;
		zb=rand()%S;
	}
}
osg::Node* MyLine3D::create3DBox(int id)
{
	initCrossBox();
	char name[100];
	sprintf(name,"%d",id);

	osg::Group* pGrp=new osg::Group;
	float radius=1.5;

	osg::Vec3f pos1=getBoxPos(radius,0,true);
	osg::Vec3f pos2=getBoxPos(radius,0,false);
	osg::Vec3f pos3=getBoxPos(radius,osg::PI,false);
	osg::Vec3f pos4=getBoxPos(radius,osg::PI,true);

	osg::Vec3d color(1,0,0);
	pGrp->addChild(createFace(pos1,pos2,pos3,pos4,color,name));

	pos1=getBoxPos(radius,osg::PI_2,true);
	pos2=getBoxPos(radius,osg::PI_2,false);
	pos3=getBoxPos(radius,-osg::PI_2,false);
	pos4=getBoxPos(radius,-osg::PI_2,true);
	pGrp->addChild(createFace(pos1,pos2,pos3,pos4,color,name));

	pGrp->addChild(create3DNode());
	return pGrp;
}
osg::Node* MyLine3D::create3DLinePoints(int id)
{
	osg::Group* pGrp=new osg::Group;
	pGrp->addChild(create3DNode());

	osg::Geode* geode=new osg::Geode;
	osg::Geometry* geom=new osg::Geometry;
	osg::Vec3Array* coords = new osg::Vec3Array;
	osg::Vec3Array* colors=new osg::Vec3Array;

	for(int i=0;i<supportPoints.size();++i)
	{
		cv::Vec3i cp=supportPoints[i];
		coords->push_back(osg::Vec3f(cp[0] ,cp[1],cp[2]));
		colors->push_back(osg::Vec3f(1,1,1));
	}
	printf("Support Points=%d, Percent =%.2f, theta=%.2f\n",coords->size(), coords->size()/(tubePoints.size()+0.0),theta);
	geom->setVertexArray(coords);

	geom->setColorArray(colors);
	geom->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,0,coords->size()));

	geode->addDrawable(geom);
	pGrp->addChild(geode);
	return pGrp;
}
osg::Vec3f MyLine3D::getBoxPos(float radius,float angle,bool up)
{
	osg::Vec3f ct(xa,ya,za);
	if(!up)	ct=osg::Vec3f(xb,yb,zb);

	float px=ct.x()+radius*(nu.x()*cos(angle)+nv.x()*sin(angle));
	float py=ct.y()+radius*(nu.y()*cos(angle)+nv.y()*sin(angle));
	float pz=ct.z()+radius*(nu.z()*cos(angle)+nv.z()*sin(angle));

	return osg::Vec3f(px,py,pz);
}
osg::Node* MyLine3D::create3DNode(osg::Vec3d color)
{
	return create3DLine(osg::Vec3d(xa,ya,za),osg::Vec3d(xb,yb,zb),osg::Vec3d(1,0,0));
}
int MyLine3D::guessRadius(const std::vector<cv::Point3i>& points)
{
	int rList[1000]={0};
	for(int i=0;i<points.size();++i)
	{
		int dist=compDirectDist(points[i].x,points[i].y,points[i].z);
		rList[dist]++;
	}
	int counter=0;
	for(int k=999;k>=1;--k)
	{
		counter+=rList[k];
		double percent=counter/(points.size()+0.0);
		if(percent>=0.1)	return k+1;
	}
	return 1;
}
double MyLine3D::compNearDist(const MyLine3D& line) const
{
	Voxel v1=line.getStartPoint();
	Voxel v2=getStartPoint();

	double dis=v1.compDist(v2);
	double minDis=dis;

	v1=line.getEndPoint();
	v2=getEndPoint();
	dis=v1.compDist(v2);
	if(dis<minDis)	minDis=dis;

	v1=line.getStartPoint();
	v2=getEndPoint();
	dis=v1.compDist(v2);
	if(dis<minDis)	minDis=dis;

	v1=line.getEndPoint();
	v2=getStartPoint();
	dis=v1.compDist(v2);
	if(dis<minDis)	minDis=dis;

	return minDis;
}
MyLine3D MyLine3D::getLineByPoints(const std::vector<cv::Point3i>& points,bool quickextend)
{
	MyLine3D mline;
	mline.setValid(false);
	if(points.size()<2)	return mline;

	cv::Vec6f line;
	cv::fitLine(points,line,CV_DIST_L2,0,0.01,0.01);  
	if(quickextend)
		mline=quickExtendLineByPoints(points,line);
	else
		mline=//getLineByLM(points,line); 
			  extendLineByPoints(points,line);

	return mline;
}
float compCurEnergy(const std::vector<cv::Point3i>& points,const MyLine3D& nline,int k)
{
	double curE=0;
	//for(int k=0;k<points.size();++k)
	{
		int cx=points[k].x;
		int cy=points[k].y;
		int cz=points[k].z;
		curE+=nline.compEnergy(cx,cy,cz);//sum all data term of point in vec_points
		//continue;
		int myID=pImg3D->getLabel(cx,cy,cz);//current point blongs to myID
		std::vector<cv::Point3i> neighbours;
		neighbours.push_back(cv::Point3i(cx-1,cy,cz));
		neighbours.push_back(cv::Point3i(cx+1,cy,cz));
		neighbours.push_back(cv::Point3i(cx,cy-1,cz));
		neighbours.push_back(cv::Point3i(cx,cy+1,cz));
		neighbours.push_back(cv::Point3i(cx,cy,cz-1));
		neighbours.push_back(cv::Point3i(cx,cy,cz+1));

		int counter1=0;
		int counter2=0;
		MyLine3D line1=nline;
		for(int t=0;t<neighbours.size();++t)
		{
			int tx=neighbours[t].x;
			int ty=neighbours[t].y;
			int tz=neighbours[t].z;
			if(pImg3D->isValidVessel(tx,ty,tz))//valid neighbour
			{
				counter1++;
				int tid=pImg3D->getLabel(tx,ty,tz);
				if(tid==myID)	continue;
				if(pGf->getLine(tid).isValid())//not outlier models
				{
					const MyLine3D& line2=pGf->getLine(tid);
		
					Voxel vp2(tx,ty,tz);
					Voxel vp1(cx,cy,cz);
					cv::Point3f tp2=line2.getProjection(vp2.x,vp2.y,vp2.s);
					cv::Point3f tp1=line1.getProjection(vp1.x,vp1.y,vp1.s);

					double dis21=line1.compDirectDist(tp2.x,tp2.y,tp2.z);
					double dis12=line2.compDirectDist(tp1.x,tp1.y,tp1.z);

					double rate=1.0/(vp1.compDist(vp2));//*vp1.compDist(vp2));
					if(rate<0.0001)	rate=0.0001;
					curE+=alpha*(dis12+dis21)*rate;
				}
				else
					curE+=SMOOTHCOST;
			}
		}
	}
	return curE;
}
MyLine3D MyLine3D::getLineByLM(const std::vector<cv::Point3i>& points,const cv::Vec6f& line)
{
	cv::Mat matJ(points.size(),7,CV_32F);//n*7
	float delta=0.5;
	MyLine3D nline;
	nline.theta=theta;

	float params[7]={xa,ya,za,xb,yb,zb,theta};
	float diff[7]={0};
	for(int k=0;k<points.size();++k)
	{
		for(int i=0;i<7;++i)
		{
			for(int j=0;j<7;++j)	diff[j]=(i==j)?-delta:0;
			nline.setStartPoint(params[0]+diff[0],params[1]+diff[1],params[2]+diff[2]);
			nline.setEndPoint(params[3]+diff[3],params[4]+diff[4],params[5]+diff[5]);
			nline.theta=params[6]+diff[6];
			float prevE=compCurEnergy(points,nline,k);

			for(int j=0;j<7;++j)	diff[j]=(i==j)?delta:0;
			nline.setStartPoint(params[0]+diff[0],params[1]+diff[1],params[2]+diff[2]);
			nline.setEndPoint(params[3]+diff[3],params[4]+diff[4],params[5]+diff[5]);
			nline.theta=params[6]+diff[6];
			float afterE=compCurEnergy(points,nline,k);
			matJ.at<float>(k,i)=afterE-prevE; 
		}
	}

	cv::Mat ep(7,7,CV_32F);
	cv::setIdentity(ep);
	cv::Mat lmat=matJ.t()*matJ + lambda*ep;

	cv::Mat matK(points.size(),1,CV_32F);//n*7
	nline.setStartPoint(params[0],params[1],params[2]);
	nline.setEndPoint(params[3],params[4],params[5]);
	nline.theta=params[6];
	for(int k=0;k<points.size();++k)
	{
		matK.at<float>(k,0)=-compCurEnergy(points,nline,k);
	}
	cv::Mat rmat=matJ.t()*matK;

	cv::Mat res;
	bool ret=cv::solve(lmat,rmat,res,CV_LU);
	for(int i=0;i<7;++i)
		params[i]=res.at<float>(i,0);
	nline.setStartPoint(xa+params[0],ya+params[1],za+params[2]);
	nline.setEndPoint(xb+params[3],yb+params[4],zb+params[5]);
	nline.theta=theta+params[6];
	//nline.setStartPoint(res.at<float>(0,0));
	return nline;
}
MyLine3D MyLine3D::extendLineByPoints(const std::vector<cv::Point3i>& points,const cv::Vec6f& line)
{
	// compute boudning box 
	MyLine3D mline;
	// bounding box
	int minX=9999,minY=9999,minZ=9999;
	int maxX=-1,maxY=-1,maxZ=-1;
	for(int i=0;i<points.size();++i)
	{
		if(points[i].x<=minX)	minX=points[i].x;
		if(points[i].x>=maxX)	maxX=points[i].x;
		if(points[i].y<=minY)	minY=points[i].y;
		if(points[i].y>=maxY)	maxY=points[i].y;
		if(points[i].z<=minZ)	minZ=points[i].z;
		if(points[i].z>=maxZ)	maxZ=points[i].z;
	}
	// let's make it a little bit bigger
	minX--;maxX++;minY--;maxY++;minZ--;maxZ++;

	// line[3-5]: postion of the line
	// line[0-2]: direction of the line
	// center points of the least points
	double px=line[3];
	double py=line[4];
	double pz=line[5];

	const float step = 1;
	std::vector<cv::Point3f> endPointsList;
	std::vector<cv::Point3f> startPointsList;
	while(px>=minX && px<=maxX && py>=minY && py<=maxY && pz>=minZ && pz<=maxZ)
	{
		endPointsList.push_back(cv::Vec3f(px,py,pz));
		px-=step*line[0];
		py-=step*line[1];
		pz-=step*line[2];
	}
	double qx=line[3]+step*line[0];
	double qy=line[4]+step*line[1];
	double qz=line[5]+step*line[2];
	while(qx>=minX&&qx<=maxX&&qy>=minY&&qy<=maxY&&qz>=minZ&&qz<=maxZ)
	{
		endPointsList.push_back(cv::Vec3f(qx,qy,qz));
		qx+=step*line[0];
		qy+=step*line[1];
		qz+=step*line[2];
	}
	/////////////////////////////////
	//brutal force search 
	if(exhaustiveSearch)
	{
		endPointsList.clear();
		for(int i=0;i<points.size();++i)
		{
			endPointsList.push_back(points[i]);
		}
		//printf("Running Exhaustive search\n");
	}
	startPointsList = endPointsList;

	// the endpoints of the line
	Voxel va(xa,ya,za), vb(xb,yb,zb);

	double minEnergy=0x7fffffff;
	for(int i=0;i<startPointsList.size();++i)
	{
		px=startPointsList[i].x;
		py=startPointsList[i].y;
		pz=startPointsList[i].z;

		float radius=3;
		if(points.size()>100)	radius=2;
		else if(points.size()<30)	radius=5;
		if(exhaustiveSearch)
		{
			Voxel vo(px,py,pz);
			if(va.compDist(vo)>radius)	continue;
		}

		for(int j=0;j<endPointsList.size();++j)
		{
			qx=endPointsList[j].x;
			qy=endPointsList[j].y;
			qz=endPointsList[j].z;

			if(exhaustiveSearch)
			{
				Voxel vo(qx,qy,qz);
				if(vb.compDist(vo)>radius)	continue;
			}

			MyLine3D nline;
			nline.setStartPoint(px,py,pz);
			nline.setEndPoint(qx,qy,qz);
			if(nline.getLength()<1.0)	continue;

			nline.theta=0.25;
			while(nline.theta<=5)
			{
				nline.theta+=0.25;
				double curE=0;
				for(int k=0;k<points.size();++k)
				{
					int cx=points[k].x;
					int cy=points[k].y;
					int cz=points[k].z;
					curE+=nline.compEnergy(cx,cy,cz);//sum all data term of point in vec_points
					//continue;
					int myID=pImg3D->getLabel(cx,cy,cz);//current point blongs to myID
					std::vector<cv::Point3i> neighbours;
					neighbours.push_back(cv::Point3i(cx-1,cy,cz));
					neighbours.push_back(cv::Point3i(cx+1,cy,cz));
					neighbours.push_back(cv::Point3i(cx,cy-1,cz));
					neighbours.push_back(cv::Point3i(cx,cy+1,cz));
					neighbours.push_back(cv::Point3i(cx,cy,cz-1));
					neighbours.push_back(cv::Point3i(cx,cy,cz+1));

					MyLine3D line1=nline;
					for(int t=0;t<neighbours.size();++t)
					{
						int tx=neighbours[t].x;
						int ty=neighbours[t].y;
						int tz=neighbours[t].z;
						if(pImg3D->isValidVessel(tx,ty,tz))//valid neighbour
						{
							int tid=pImg3D->getLabel(tx,ty,tz);
							if(tid==myID)	continue;
							if(pGf->getLine(tid).isValid())//not outlier models
							{
								//const MyLine3D& line1=pGf->getLine(myID);
								const MyLine3D& line2=pGf->getLine(tid);
		
								Voxel vp2(tx,ty,tz);
								Voxel vp1(cx,cy,cz);
								cv::Point3f tp2=line2.getProjection(vp2.x,vp2.y,vp2.s);
								cv::Point3f tp1=line1.getProjection(vp1.x,vp1.y,vp1.s);

								double dis21=line1.compDirectDist(tp2.x,tp2.y,tp2.z);
								double dis12=line2.compDirectDist(tp1.x,tp1.y,tp1.z);

								double rate=1.0/(vp1.compDist(vp2));//*vp1.compDist(vp2));
								if(rate<0.0001)	rate=0.0001;
								curE+=alpha*(dis12+dis21)*rate;
								//curE+=beta*line1.compNearDist(line2);
							}
							else
								curE+=SMOOTHCOST;
						}
					}
					
				}
				if(curE<minEnergy)
				{
					minEnergy=curE;
					mline=nline;//keep this line
				}
			}
		}
	}
	mline.setValid(true);	
	//printf("Leave extendLineByPoints\n");
	return mline;
}
MyLine3D MyLine3D::quickExtendLineByPoints(const std::vector<cv::Point3i>& points,const cv::Vec6f& line)
{
	MyLine3D mline;
	int minX=9999,minY=9999,minZ=9999;
	int maxX=-1,maxY=-1,maxZ=-1;
	for(int i=0;i<points.size();++i)
	{
		if(points[i].x<=minX)	minX=points[i].x;
		if(points[i].x>=maxX)	maxX=points[i].x;
		if(points[i].y<=minY)	minY=points[i].y;
		if(points[i].y>=maxY)	maxY=points[i].y;
		if(points[i].z<=minZ)	minZ=points[i].z;
		if(points[i].z>=maxZ)	maxZ=points[i].z;
	}
	minX--;maxX++;minY--;maxY++;minZ--;maxZ++;
	//printf("Adjust fitting points %d,%.2f,%.2f,%.2f,%.2f, minX=%d,maxX=%d,minY=%d,maxY=%d\n",points.size(),line[0],line[1],line[2],line[3],minX,maxX,minY,maxY);
	double px=line[3];
	double py=line[4];
	double pz=line[5];
	double qx=line[3];
	double qy=line[4];
	double qz=line[5];

	MyLine3D nline;
	int prevCounter=0;
	nline.setStartPoint(px,py,pz);
	nline.setEndPoint(qx,qy,qz);
	nline.mRadius=3;
	const int step=2;
	while(px>=minX&&px<=maxX&&py>=minY&&py<=maxY&&pz>=minZ&&pz<=maxZ)
	{
		px-=step*line[0];
		py-=step*line[1];
		pz-=step*line[2];

		nline.setStartPoint(px,py,pz);
		int counter=0;
		for(int j=0;j<points.size();++j)
		{
			if(nline.inLineRange(points[j].x,points[j].y,points[j].z))	++counter;
		}
		if(counter<=prevCounter)
		{
			if(useShortLines)	break;
		}
		else
			prevCounter=counter;
	}
	px+=step*line[0];
	py+=step*line[1];
	pz+=step*line[2];

	mline.setStartPoint(px,py,pz);
	while(qx>=minX&&qx<=maxX&&qy>=minY&&qy<=maxY&&qz>=minZ&&qz<=maxZ)
	{
		qx+=step*line[0];
		qy+=step*line[1];
		qz+=step*line[2];

		nline.setEndPoint(qx,qy,qz);
		int counter=0;
		for(int j=0;j<points.size();++j)
		{
			if(nline.inLineRange(points[j].x,points[j].y,points[j].z))	++counter;
		}
		if(counter<=prevCounter)
		{
			if(useShortLines)	break;
		}
		else
			prevCounter=counter;
	}
	qx-=step*line[0];
	qy-=step*line[1];
	qz-=step*line[2];
	mline.setEndPoint(qx,qy,qz);
	mline.setValid(true);	
	return mline;
}
void MyLine3D::adjustLine(const std::vector<cv::Point3i>& cpoints,const CImageData& img3D )
{
	MyLine3D newline=getLineByPoints(cpoints,false);
	if(newline.isValid())	setByLine(newline);//set by this new valid line
	return ;
}
std::vector<cv::Point3i> MyLine3D::purgePoints(const std::vector<cv::Point3i>& cpoints)
{
	cv::Mat data(cpoints.size(),3, CV_32F),labels;
	int all_keypoints=cpoints.size();
	for(int i=0;i<all_keypoints;++i)
	{
		data.at<float>(i,0)=cpoints[i].x; 
			//compDirectDist(cpoints[i].x,cpoints[i].y,cpoints[i].z);
		data.at<float>(i,1)=cpoints[i].y; 
		data.at<float>(i,2)=cpoints[i].z;
	}
	const int K=4;
	const int MAX_ITER=100;
	if(all_keypoints<=K)	return cpoints;
	printf("kmeans begin K=%d, Number=%d\n",K,all_keypoints);
	cv::Mat centers(K, 1,CV_32F);
	kmeans(data, K, labels,cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, MAX_ITER, 1.0),3, 2, centers);

	int counter[K]={0};
	for(int i=0;i<all_keypoints;++i)
		++counter[labels.at<int>(i,0)];

	int smallLabel=-1;
	int minNum=0x7fffffff;
	for(int i=0;i<K;++i)
	{
		if(counter[i]<minNum)
		{
			minNum=counter[i];
			smallLabel=i;
		}
	}

	double percent=minNum/(all_keypoints+0.0);
	if(percent<0.2)//too low
	{
		std::vector<cv::Point3i> points;
		for(int i=0;i<all_keypoints;++i)
		{
			if(smallLabel!=labels.at<int>(i,0))
				points.push_back(cpoints[i]);
		}
		printf("KMeans Purge From %d to %d\n",cpoints.size(),points.size());
		return points;
	}
	else	
		return cpoints;
}
bool MyLine3D::isSimilar(const MyLine3D& line)
{
	double angle=compAngle(line);
	if(angle<cos(osg::PI_4/2))	return false;

	Voxel v1=line.getStartPoint();
	Voxel v2=this->getStartPoint();
	double dis1=v1.compDist(v2);

	v1=line.getStartPoint();
	v2=this->getEndPoint();
	double dis2=v1.compDist(v2);

	v1=line.getEndPoint();
	v2=this->getEndPoint();
	double dis3=v1.compDist(v2);

	v1=line.getEndPoint();
	v2=this->getStartPoint();
	double dis4=v1.compDist(v2);

	double maxDis=(std::max)((std::max)(dis1,dis2),(std::max)(dis3,dis4));

	double len1=line.getLength();
	double len2=this->getLength();

	return maxDis>=0.35*(len1+len2);
}
double MyLine3D::getSimilarity(const MyLine3D& line)
{
	Voxel v1=line.getStartPoint();
	Voxel v2=this->getStartPoint();
	double dis1=v1.compDist(v2);

	v1=line.getStartPoint();
	v2=this->getEndPoint();
	double dis2=v1.compDist(v2);

	v1=line.getEndPoint();
	v2=this->getEndPoint();
	double dis3=v1.compDist(v2);

	v1=line.getEndPoint();
	v2=this->getStartPoint();
	double dis4=v1.compDist(v2);

	return (std::min)((std::min)(dis1,dis2),(std::min)(dis3,dis4));
}
void MyLine3D::kmeanLines(const std::vector<cv::Point3i>& cpoints,const CImageData& img3D,bool  keepVoxels)
{
	cv::Mat data(cpoints.size(),3, CV_32F);
	int all_keypoints=cpoints.size();
	for(int i=0;i<all_keypoints;++i)
	{
		data.at<float>(i,0)=cpoints[i].x; 
		data.at<float>(i,1)=cpoints[i].y; 
		data.at<float>(i,2)=cpoints[i].z;
	}
	for(int K=2;K<=12;K+=2)
	{
		if(cpoints.size()>K)
		{
			cv::Mat labels=doKmeansPoints(data,K);
			std::vector<cv::Point3i> pointsList[10000];

			for(int i=0;i<cpoints.size();++i)
			{
				pointsList[labels.at<int>(i,0)].push_back(cpoints[i]);
			}
			clusterLines.clear();//clear line list
			for(int i=0;i<K;++i)
			{
				MyLine3D newline=getLineByPoints(pointsList[i],true);
				//newline.adjustLine(pointsList[i],img3D);//set sigma, mRadius
				if(newline.isValid() && newline.getLength()>1.0)
				{
					clusterLines.push_back(newline);
					if(keepVoxels)
						clusterLines.back().supportPoints=pointsList[i];
				}
			}
		}
	}
	printf("Original Line has %d points, after kmeans clustring,there are  %d lines\n",cpoints.size(),clusterLines.size());
}
cv::Mat MyLine3D::doKmeansPoints(const cv::Mat& data,const int K)
{
	cv::Mat labels;
	int all_keypoints=data.rows;
	const int MAX_ITER=100;

	if(all_keypoints<=K)	return labels;

	printf("kmeans begin K=%d, Number=%d\n",K,all_keypoints);
	cv::Mat centers(K, 1,CV_32F);
	kmeans(data, K, labels,cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, MAX_ITER, 1.0),3, 2, centers);

	return labels;
}